﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace NinjaToolbox
{
    public static class PhysicsUtil
    {
        public static readonly float Infinity = 100000.0f;


        public static Vector3 StepMove(Vector3 current, Vector3 to, int layerMask)
        {
            var hit = default(RaycastHit);
            var relativePosition = (to - current);
            var direction = relativePosition.normalized;
            var maxDistance = relativePosition.magnitude;

            var isHit = Physics.Raycast(new Ray(current, direction), out hit, maxDistance, layerMask);

            if (isHit)
            {
                return hit.point;
            }
            else
            {
                return to;
            }
        }

        public static bool CapsuleHitOther(CapsuleShape capsule, PhysicsObjectShapeBase other, out Vector3 referencePoint)
        {
            var result = false;

            var up = Vector3.up;
            var right = Vector3.right;
            var forward = Vector3.forward;

            referencePoint = Vector3.zero;

            switch (capsule.direction)
            {
                case CapsuleShape.DirectionEnum.X:
                    up = Vector3.right;
                    right = Vector3.forward;
                    forward = Vector3.up;
                    break;
                case CapsuleShape.DirectionEnum.Z:
                    up = Vector3.forward;
                    right = Vector3.up;
                    forward = Vector3.right;
                    break;
            }

            const float ERROR = 0.01f;
            var remainHeight = Mathf.Max(capsule.height - capsule.radius * 2f, 0) + ERROR;
            var remainHeightHalf = remainHeight * 0.5f;
            var xPillarOrigin = capsule.center + up * -remainHeightHalf;
            var xPillarEnd = capsule.center + up * remainHeightHalf;

            var a = other.MassPointToEdge(other.GetWorldPosition(), capsule.transform.localToWorldMatrix.MultiplyPoint3x4(xPillarOrigin));
            referencePoint = a;
            a = capsule.transform.worldToLocalMatrix.MultiplyPoint3x4(a);

            if (Vector3.Distance(xPillarOrigin, a) <= capsule.radius)
                result = true;

            if (!result)
            {
                var b = other.MassPointToEdge(other.GetWorldPosition(), capsule.transform.localToWorldMatrix.MultiplyPoint3x4(xPillarEnd));
                referencePoint = b;
                b = capsule.transform.worldToLocalMatrix.MultiplyPoint3x4(b);

                if (Vector3.Distance(xPillarEnd, b) <= capsule.radius)
                    result = true;
            }

            if (!result)
            {
                var localCompareMassPoint = capsule.transform.worldToLocalMatrix.MultiplyPoint3x4(other.GetWorldPosition());
                var abProjectionPoint = xPillarOrigin + Vector3.Project(localCompareMassPoint - xPillarOrigin, (xPillarEnd - xPillarOrigin).normalized);
                abProjectionPoint = capsule.transform.localToWorldMatrix.MultiplyPoint3x4(abProjectionPoint);

                referencePoint = other.MassPointToEdge(other.GetWorldPosition(), abProjectionPoint);

                if (capsule.IsHit(referencePoint))
                    result = true;
            }

            return result;
        }
    }
}
